package simulation;

import java.io.IOException;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.Map;
import java.util.Timer;
import java.util.TimerTask;
import java.util.concurrent.Semaphore;

import maze.Location;
import maze.Maze;
import pacman.Ghost;
import pacman.Pacman;

public class Simulator {
	private Maze maze;
	private ColorMap colormap;
	private ArrayList<Ghost> ghosts = new ArrayList<Ghost>();
	private Map<Ghost, Location> ghostLocations;
	private Map<Ghost, Integer> ghostRotations; //degrees
	private Map<Ghost, Semaphore> ghostLocks;
	private Map<Ghost, Semaphore> ghostTravelLocks;
	
	private Pacman pacman;
	
	public float speed = 350; //wheel-turnspeed of the ghosts, degree/s 		MUST BE THE SAME AS THE SPEED OF THE REAL ROBOT!!
	private float wheelCirc = 15; //circumference of a wheel, cm				MUST BE THE SAME AS THE CIRC OF THE REAL ROBOT!!
	private float wheelBase = 15; //distance between robot wheels, cm			MUST BE THE SAME AS THE BASE OF THE REAL ROBOT!!
	
	int period = 10;
	
	public Simulator(Maze m, ColorMap c){
		maze = m;
		colormap = c;
		ghostLocations=new HashMap<Ghost, Location>();
		ghostRotations = new HashMap<Ghost, Integer>();
		ghostLocks = new HashMap<Ghost, Semaphore>();
		
	}
	
	public Maze getMaze(){
		return maze;
	}
	
	public ColorMap getColorMap(){
		return colormap;
	}
	
	public Map<Ghost, Location> getGhostLocations(){
		return ghostLocations;
	}
	
	public Map<Ghost, Integer> getGhostRotations(){
		return ghostRotations;
	}
	
	public Pacman getPacman() {
		return pacman;
	}

	public void setPacman(Pacman pacman) {
		this.pacman = pacman;
	}
	
	public void addGhost(Ghost ghost, Location loc, int rotation){
		ghosts.add(ghost);
		ghostLocations.put(ghost, loc);
		ghostRotations.put(ghost, rotation);
		ghostLocks.put(ghost, new Semaphore(1));
		try {
			ghostLocks.get(ghost).acquire();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		ghost.setSimulator(this);
	}
	
	public int SimulateLightSensor(Ghost ghost)
	{
		Location loc = ghostLocations.get(ghost);
		return colormap.GetSensorReading((int)(loc.x+0.5f), (int)(loc.y+0.5f));
	}
	
	public int SimulateUltrasonicSensor(Ghost ghost)
	{
		return maze.getMaxDistance(ghostLocations.get(ghost), ghostRotations.get(ghost)-ghost.getSensorRotation());
	}
	
	/**
	 * De direction wordt genummerd van links naar rechts.  Er zijn 9 mogelijke sectoren waarin pacman
	 * zich kan bevinden, gaande van 120° tot -120°
	 */
	public int SimulateInfraredSensor(Ghost ghost)
	{
		//beperking op de afstand die de sensor kan zien...
		int maxDistanceSensor = 60;
		
		/*
		 * rotatie 0 is naar rechts kijken
		 *        1 2 3
		 *              4
		 *     0  Jos   5 
		 *              6
		 *        9 8 7
		 */
		Location pacmanLocation = getPacman().getLocation();
		//verplaatsingsvector
		Location ghostLocation = ghostLocations.get(ghost);
//		double rico = (pacmanLocation.getY() - ghostLocation.getY())/(pacmanLocation.getX() - ghostLocation.getX());
//		double angle = Math.atan(rico)*(180/Math.PI) + getGhost().getRotation();
//		// bepalen we de sector waarin pacman zich bevind voor de infraroodsensor
//		double sectorDouble = (-angle+120)/26.66667;
//		int sector = (int) (Math.ceil(sectorDouble));
//		//Vanaf hier proberen we de maxdistance uit te voeren en te vergelijken met de afstand dat pacman echt is 
//		// ten opzichte van de robot: dit op basis van afstandsberekening tussen 2 punten
//		int IntAngle = (int) angle;
//		double SqrtXDist = (pacmanLocation.getX()-ghostLocation.getX())*(pacmanLocation.getX()-ghostLocation.getX());
//		double SqrtYDist = (pacmanLocation.getY()-ghostLocation.getY())*(pacmanLocation.getY()-ghostLocation.getY());
//		int distanceCalculated = (int) Math.sqrt(SqrtXDist + SqrtYDist);
//		int maxDistance = getGhost().getMaze().getMaxDistance(ghostLocation, IntAngle);
//		if(  maxDistance < distanceCalculated){
//			sector = 0;
//		}
//			
//		return sector;
		double diffx = pacmanLocation.x - ghostLocation.x;
		double diffy = pacmanLocation.y - ghostLocation.y;
		//verplaatsingsvector draaien (zoek op: "2d rotation matrix) om te compenseren voor draaiing robot
		double counterAngle = -ghostRotations.get(ghost)*Math.PI/180.0; //in radialen
		double temp = Math.cos(counterAngle)*diffx - Math.sin(counterAngle)*diffy;
		diffy = Math.sin(counterAngle)*diffx + Math.cos(counterAngle)*diffy;
		diffx = temp;
		//afstand uitrekenenen en bewaren, verplaatsingsvector normaliseren (behoud de richting)
		double distance = Math.sqrt(diffx*diffx + diffy*diffy);
		diffx /= distance;
		diffy /= distance;
		int direction = 0;
		double dirWidth = 240.0/9.0;
		//bepalen van de richting
		if(diffx<Math.cos(120.0*Math.PI/180.0)) { //~120
			direction = 0;
		} else if(diffx<Math.cos((120.0-dirWidth)*Math.PI/180.0)) { //~84
			direction = (diffy>0.0)?1:9;
		} else if(diffx<Math.cos((120.0-dirWidth*2)*Math.PI/180.0)) { //~58
			direction = (diffy>0.0)?2:8;
		} else if(diffx<Math.cos((120.0-dirWidth*3)*Math.PI/180.0)) { //~31
			direction = (diffy>0.0)?3:7;
		} else if(diffy>Math.sin((120.0-dirWidth*4)*Math.PI/180.0)) { //overschakelen op sinus voor nauwkeurigheid
			direction = 4;
		} else if(diffy<Math.sin((120.0-dirWidth*5)*Math.PI/180.0)) { //overschakelen op sinus voor nauwkeurigheid
			direction = 6;
		} else direction = 5;
		double angle = ((Math.abs(direction-5)>=3)?Math.acos(diffx):Math.asin(diffy))*180/Math.PI; //calculate angle, kies cos of sin voor beste nauwkeurigheid
		if(Math.abs(direction-5)>=3) { if(Math.asin(diffy)<0.0) angle = -angle; }
		else { if(Math.acos(diffx)<0.0) angle = 180.0-angle; }
		int maxDistance = maze.getMaxDistance(ghostLocation, (int)angle + ghostRotations.get(ghost));
		if(  maxDistance < distance ){
			direction = 0;
		}	
		return direction;
	}
	
	/**
	 * Simulates the traveling of a robot.
	 * @param ghost
	 * @param distance	cm
	 * @throws IOException 
	 */
	public void SimulateTravel(Ghost ghost, int distance) throws IOException
	{
		/*try {
			ghostLocks.get(ghost).acquire();//this will block if another rotate/travel task is busy
		} catch(InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		} */
		while(ghost.isTravelling);
		Timer timer = new Timer();
		int period = 10; //milliseconds SHOULD BE SMALL ENOUGH (otherwise, lightsensor will not read barcodes accurately)
		float travelSpeed = speed * wheelCirc/360;
		
		TravelTask travelTask = new TravelTask(ghost, distance, travelSpeed, period);
		timer.schedule(travelTask, 0, period);
		try {
			ghostLocks.get(ghost).acquire();//this blocks until the task is done
		} catch(InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		}
		
		ghost.sendPOSITION();
	}
	
	/**
	 * Simulates the rotation (in place) of a robot.
	 * @param ghost
	 * @param angle	degr
	 */
	public void SimulateRotation(Ghost ghost, int angle)
	{
		/*try {
			ghostLocks.get(ghost).acquire();
		} catch(InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		}*/
		while(ghost.isTravelling);
		Timer timer = new Timer();
		int period = 10; //milliseconds
		float turnSpeed = (float) (((wheelCirc)*(speed))/((Math.PI)*(wheelBase)));
		
		RotateTask rotateTask = new RotateTask(ghost, angle, turnSpeed, period);
		timer.schedule(rotateTask, 0, period);
		try {
			ghostLocks.get(ghost).acquire();
		} catch(InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		}
	}
	
	
	class TravelTask extends TimerTask {

		private Ghost ghost;
		private float dist;
		private double startX;
		private double startY;
		private int rot;
		private int remainingTicks;
		private int totalTicks;
		private float distancePerTick;
		
		/**
		 * Constructor
		 * @param g
		 * @param distance 				cm
		 * @param travelspeed			cm/s
		 * @param updateIntervalMilli	millisec
		 */
		public TravelTask(Ghost g, float distance, float travelspeed, float updateIntervalMilli)
		{
			ghost = g;
			startX = ghostLocations.get(ghost).x;
			startY = ghostLocations.get(ghost).y;
			rot = ghostRotations.get(ghost);
			dist = distance;
			
			distancePerTick = travelspeed * updateIntervalMilli/1000;
			totalTicks = Math.abs((int) (distance/distancePerTick)); //= totaltime/timepertick
			remainingTicks = totalTicks;
		}
		
		@Override
		public void run() {
			ghost.isTravelling = true;
			rot = ghostRotations.get(ghost);
			if(remainingTicks > 0)
			{
				
				int neg = 1; // -1 if negative distance +1 if positive
				if (dist<0)
					neg = -1;
				Location prev = ghostLocations.get(ghost);
				double newX;
				double newY;
				double rotRad = Math.toRadians(rot);				
				newX = prev.x + neg*(Math.cos(rotRad)*distancePerTick);
				newY = prev.y + neg*(Math.sin(rotRad)*distancePerTick);
				
				Location newLoc = new Location(newX, newY);
				ghostLocations.put(ghost, newLoc);
				
				
				if(dist<0 && (float)maze.getMaxDistance(ghostLocations.get(ghost), rot+180) <= 2*distancePerTick )
				{
					
					rotRad = Math.toRadians(rot);				
					newX = newX + neg*(Math.cos(rotRad)*(float)(maze.getMaxDistance(ghostLocations.get(ghost), rot+180))-0.1);
					newY = newY + neg*(Math.sin(rotRad)*(float)(maze.getMaxDistance(ghostLocations.get(ghost), rot+180))-0.1);
					
					newLoc = new Location(newX, newY);
					ghostLocations.put(ghost, newLoc);
					ghostLocks.get(ghost).release();
					ghost.isTravelling = false;
					this.cancel();
				}
				if(dist>0 && (float)maze.getMaxDistance(ghostLocations.get(ghost), rot) <= 2*distancePerTick)
				{
					rotRad = Math.toRadians(rot);				
					newX = newX + neg*(Math.cos(rotRad)*(float)(maze.getMaxDistance(ghostLocations.get(ghost), rot))-0.1);
					newY = newY + neg*(Math.sin(rotRad)*(float)(maze.getMaxDistance(ghostLocations.get(ghost), rot))-0.1);
						
					newLoc = new Location(newX, newY);
					ghostLocations.put(ghost, newLoc);
					ghostLocks.get(ghost).release();
					ghost.isTravelling = false;
					this.cancel();
				}
				remainingTicks--;
			}
			else
			{
				/*int neg = 1; // -1 if negative distance +1 if positive
				if (dist<0)
					neg = -1;
				//set final pos
				Location prev = ghostLocations.get(ghost);
				double newX;
				double newY;
				double rotRad = Math.toRadians(rot);				
				newX = prev.x + neg*(Math.cos(rotRad)*distancePerTick);//(float)(maze.getMaxDistance(ghostLocations.get(ghost), rot))-0.1);
				newY = prev.x + neg*(Math.sin(rotRad)*distancePerTick);//(float)(maze.getMaxDistance(ghostLocations.get(ghost), rot))-0.1);
					
				Location newLoc = new Location(newX, newY);
				ghostLocations.put(ghost, newLoc);
				*/
				ghost.isTravelling = false;
				ghostLocks.get(ghost).release();
				this.cancel();
			}
		
		}
	}
	
	class RotateTask extends TimerTask {

		private Ghost ghost;
		private int angle;
		private int startAngle;
		private int remainingTicks;
		private int totalTicks;
		private float degreesPerTick;
		
		/**
		 * Constructor
		 * @param g
		 * @param angle					degrees to turn CCW
		 * @param turnspeed				degrees/s
		 * @param updateIntervalMilli	milliseconds
		 */
		public RotateTask(Ghost g, int angle, double turnspeed, int updateIntervalMilli)
		{
			ghost = g;
			startAngle = ghostRotations.get(ghost);
			this.angle = angle;
			
			degreesPerTick = (float) (turnspeed * (float)(updateIntervalMilli)/1000.0);
 			totalTicks = (int)(Math.abs(angle)/degreesPerTick);
			remainingTicks = totalTicks;
		}
		
		@Override
		public void run() {
			ghost.isTravelling = true;
			if(remainingTicks > 0)
			{
				int newAngle = (int)(startAngle + Math.signum(angle)*degreesPerTick*(totalTicks-remainingTicks));
				ghostRotations.put(ghost, newAngle%360);
				
				remainingTicks--;
			}
			else
			{
				ghostRotations.put(ghost, (startAngle + angle)%360);
				ghost.isTravelling = false;
				ghostLocks.get(ghost).release();
				this.cancel();
			}
			
		}
		
	}
}
